package estrategia;

import java.io.IOException;

import javax.vecmath.Point3d;

public class CamposPotencialUnivectorial {
	
	private Utiles utiles;
	
	// Radio de repulsion definido para los obstaculos
	private final double R = 0.35;
	// Radio de repulsion definido para las obstaculos
	private final double R_paredes = 0.20;
	// Tamano del anillo que rodea el radio del robot, es aprox el grosor del robot
	private final double M0 = 0.35;
	// Valores de las paredes
	private final double PARED_ARRIBA = 1.7;
	private final double PARED_ABAJO = 0;
	private final double PARED_IZQ = 0;
	private final double PARED_DER = 2.1;

	
	public CamposPotencialUnivectorial(Utiles u) {
		utiles = u;
	}

	/*** Realiza la suma de vectores ***/
	public Vector sumaVectores(Vector vectorA, Vector vectorB){
		// Pasamos de polar a escalar
		if(vectorA.magnitud==0){ return vectorB;}
		if(vectorB.magnitud==0){ return vectorA;}
		//double xa = (double)(Math.cos(vectorA.getDireccion() * Math.PI/180)*vectorA.getMagnitud());
		//double xb = (double)(Math.cos(vectorB.getDireccion() * Math.PI/180)*vectorB.getMagnitud());
		double xa = (double)(Math.cos(vectorA.getDireccion())*vectorA.getMagnitud());
		double xb = (double)(Math.cos(vectorB.getDireccion())*vectorB.getMagnitud());
		
		//double ya = (double)(Math.sin(vectorA.getDireccion() * Math.PI/180)*vectorA.getMagnitud());
		//double yb = (double)(Math.sin(vectorB.getDireccion() * Math.PI/180)*vectorB.getMagnitud());
		double ya = (double)(Math.sin(vectorA.getDireccion())*vectorA.getMagnitud());
		double yb = (double)(Math.sin(vectorB.getDireccion())*vectorB.getMagnitud());
		
		double xt = xa + xb;
		double yt = ya + yb;
		
		// pasamos de escalar a polar
		double radianes = 0;
		if (xt == 0) {
			radianes = Math.signum(yt)*Math.PI/2;
		} else 
			radianes = (double)(Math.atan(yt/xt));
		double direccion = radianes * (180/Math.PI);
		// Averiguamos el cuadrante
		if((xt>=0) && (yt>=0)){
			// Primer cuadrante
			//direccion = direccion;
		} else if ((xt<0)&&(yt>=0)) {
			// Segundo cuadrante
			direccion = 180 + direccion;			
		} else if ((xt<0)&&(yt<0)){
			// Tercer cuadrante
			direccion = -180 + direccion;
		} else {
			// Cuarto cuadrante
			direccion = direccion;
		}
			
		double magnitud = (double)(Math.sqrt((xt*xt)+(yt*yt)));
		
		Vector vectorSuma = new Vector(magnitud, Math.toRadians(direccion));
		return vectorSuma;
	}
	
	
	/*** Devuelve los puntos de la pared ***/
	public Point3d[] puntosPared(Point3d p) {
		Point3d[] paredes = new Point3d [4];
		
		paredes[0] = new Point3d(PARED_IZQ, p.y, 0);
		paredes[1] = new Point3d(p.x, PARED_ARRIBA, 0);
		paredes[2] = new Point3d(PARED_DER, p.y, 0);
		paredes[3] = new Point3d(p.x, PARED_ABAJO, 0);
		
		return paredes;
	}
	
	
	/*** Calcula la direccion del campo atractivo ***/
	public Vector atraccionUnivectorial(Point3d p, Point3d g, Point3d r) {
		double theta_pg = utiles.getAnguloTheta(p, g);
		double theta_pr = utiles.getAnguloTheta(p, r);
		double alpha = theta_pr - theta_pg;
		
		// NUEVO
		double betha = theta_pg - alpha;
		//if (betha < Math.PI)
			//betha = Math.PI + (betha + Math.PI);
		//double direccion = 0;
		//if (p.z < 0)
			//direccion = betha + p.z;
		//else 
		double direccion = betha - p.z;
		
		return new Vector(1, direccion);				
	}
	
	/*** Calcula el angulo resultante del campo ***/
	public double hallarAnguloVectorResultante(double R, double L, double M0, Point3d p, Point3d g) {
		double angulo = 0;
		if (R <= L) {
			angulo = Math.asin(R/L);
		} else {
			angulo = Math.atan2(R + M0, L);
		}
		
		return angulo;
	}
	
	
	/*** Calcula las fuerzas repulsivas de todos los obstaculos ***/
	public Vector repulsionUnivectorial(Point3d p, Point3d g, Point3d o) {
		// Distancia entre un obstaculo y el robot
		double L = 0;
		// Angulo formado pro la posicion del objetivo y el robot
		double theta_g = 0;
		// Indica el angulo entre la posicion del robot y la del onbstaculo
		double theta_l = 0;
		// Se calcula obteniendo el angulo entre la linea tangente al circulo de repulsion y la posicion del robot
		double theta_2 = 0;
		// Vector resultado
		Vector resultante = null;
		
		// Calculamos los angulos
		theta_l = utiles.getAnguloTheta(p, o);
		theta_g = utiles.getAnguloTheta(p, g);
		L = utiles.distanciaEuclidea(p, o);
		theta_2 = hallarAnguloVectorResultante(R, L, M0, p, g);
		// Calculamos la direccion del vector resultante
		// Si pasa esto es proque el objetivo esta atras de un obstaculo
		double direccion = 0;
		if (Math.signum(theta_l - theta_g) == 0) {
			direccion = theta_2 + p.z;
		} else {
			direccion = theta_l - Math.signum(theta_l - theta_g)*theta_2;
		}
		 
		resultante = new Vector(1, direccion);
		
		return resultante;				
	}
	
	
	//Asume que se detecto colision sino revienta
	public Vector calcularVectorRepulsion(Point3d p, Point3d g, Point3d[] obstaculos, Point3d[] paredes) {
		Vector resultante = null;
		for (int i= 0; i<4; i++) {
			if (posibleColisionPared(p, g, paredes[i])) {
				resultante = repulsionUnivectorial(p, g, paredes[i]);
				break;
			}
		}
		
		for (int i=0; i<obstaculos.length; i++){
			if (posibleColisionObstaculo(p, g, obstaculos[i])) {
				resultante = repulsionUnivectorial(p, g, obstaculos[i]);
				break;
			} 
		}
		
		return resultante;
	}
	
	
	/*** Devuelve True si hay posible colision ***/
	public boolean posibleColisionObstaculo(Point3d p, Point3d g, Point3d o) {
		double L = utiles.distanciaEuclidea(p, g);
		double D = utiles.distanciaEuclidea(o, g);
		double distObs = utiles.distanciaEuclidea(p, o);
		double theta_pg = utiles.getAnguloTheta(p, g);
		double theta_pr = utiles.getAnguloTheta(p, o);
		double alpha = utiles.getDiferenciaAngulos(theta_pg, theta_pr);
		
		if (L<=D) {
			return false;
		} else {
			double d = Math.sin(alpha)*L;
			if ((d <= R && distObs <= M0 + R ) ||  distObs <= R)
				return true;
			else {
				return false;
			}
		}
	}
	
	
	/*** Devuelve True si hay posible colision ***/
	public boolean posibleColisionPared(Point3d p, Point3d g, Point3d o) {
		double L = utiles.distanciaEuclidea(p, g);
		double D = utiles.distanciaEuclidea(o, g);
		double distObs = utiles.distanciaEuclidea(p, o);
		double theta_pg = utiles.getAnguloTheta(p, g);
		double theta_pr = utiles.getAnguloTheta(p, o);
		double alpha = utiles.getDiferenciaAngulos(theta_pg, theta_pr);
		
		
		double d = Math.sin(alpha)*L;
		if ((d <= R_paredes && distObs <= M0 + R_paredes ) ||  distObs <= R_paredes)
			return true;
		else {
			return false;
		}
	}
	
	
	/*** Devuelve la direccion resultante del campo ***/
	public Vector hallarDireccionCampo(Point3d p, Point3d g, Point3d r, Point3d[] obstaculos) throws IOException {
		Vector resultante = null;
		Point3d[] paredes = puntosPared(p);
		boolean posibleColision = false;
		
		for (int i=0; i<paredes.length; i++) {
			if (posibleColisionPared(p, g, paredes[i])) {
				posibleColision = true;
				resultante = repulsionUnivectorial(p, g, paredes[i]);
				break;
			}		
		}
		
		for (int i=0; i<obstaculos.length; i++) {
			if (posibleColisionObstaculo(p, g, obstaculos[i])) {
				posibleColision = true;
				resultante = repulsionUnivectorial(p, g, obstaculos[i]);
				break;
			}		
		}
		
		if (! posibleColision) {
			resultante = atraccionUnivectorial(p, g, r);
		}
		
		return resultante;
	}
}
